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Abstract 



An autonomous flying robot was designed, built and tested which uses mag- 
netometers as the source of attitude information. The complete control sys- 
tem was designed and built, including the electronics and the software re- 
quired for a first order autonomous flight. The flight controller is capable 
of being installed on many airframes. It was successfully tested on a small 
electric-powered model aircraft controlling both heading and pitch. 
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Chapter 1 
Introduction 



Unmanned Air Vehicles (UAV) control systems appear to have begun de- 
velopment primarily for military purposes only a few years after the first 
manned flight of 1903. UAV technology began to take shape driven by the 
first world war when in 1917 the Sperry Aerial Torpedo developed in the 
USA used a gyroscopic stabilizer to fly 50 miles [1], becoming the first UAV 
by flying via remote control. 

The reliance on radio control was removed a year later in 1918 with the 
Kettering Aerial Torpedo. Invented by Charles F. Kettering, developed for 
the U.S. military and built by Dayton- Wright Airplane Company, the aircraft 
took off from a trolley running along a track. Stabilized in flight by pre- 
set vacuum pneumatic and electrical controls the Kettering Aerial Torpedo 
would turn off its engines and release its wings, diving toward its target after 
a predetermined amount of time. 

In the decade following the First World War not much more would be 
developed and investigated due to the armistice and a scarcity of funds. 
The next major developmental step would not come until the Second World 
War with the launch of the Fi-103, better known as the Vergeltungswaffe 
or V-l in 1944. The V-l was powered by a pulse-jet carrying a one ton 
warhead traveling around 560kph at 4,000ft with a range of 240km. This 
prompted America to develop their own long range UAV, the PB4Y-1 and 
BQ-7 which where converted Liberators and B-17's. These carried 25,000 
pounds of explosives, flew via remote control using camera guidance systems, 
and were successful in knocking out V-l launch sites - marking the first time 
UAV's were used against each other. 

The Vietnam and Cold war in the 1960's, 70's and 80's saw a change 
in the role of UAV's. They moved from target drones and RPV (Remotely 
Piloted Vehicle) combat vehicles to reconnaissance vehicles. An example of 
this is the Lockheed D-21 from 1965. This was designed to be "stealth" and 
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had a top speed of Mach 4 and a ceiling of 80,000 feet. 

UAV's continued their military involvement through the 90's. The 1990's 
realized the commercial viability of UAV's. The demand for technology items 
such as airbags in cars required high speed, low cost, small sensors with low 
power consumption that could also be used for unmanned flight. As peo- 
ple needed more information about the environment with greater accuracy 
UAV's such as the Pathfinder were developed, capable of flying for long pe- 
riods of time at high altitudes collecting wind and weather information with 
very fine sensors and using high resolution digital cameras. One exciting 
development is Helios. Helios is intended to also be used as a broadband 
communications platform as a complement to satellite and terrestrial com- 
munication systems by flying continuously at 50,000 - 70,000 feet for months 
at a time. 

As the century turned the technology required for autonomous flight 
rapidly became cheaper. Because of this the past two years have seen a 
rapid growth in autonomous projects. Today there are so many in develop- 
ment, with so much talk about them that a Goggle search for "UAV" returns 
1,850,000 results. UAV's have recently been commissioned for disaster relief 
efforts, traffic monitoring, aerial mapping, and much more. Development 
groups include universities, aeronautical companies, military, and a few hob- 
byist groups worldwide. 

The project name for this control system (Apeliotes) is the name of the 
God of the south east winds, from Greek mythology. Apeliotes is often 
depicted wearing gumboots with curly hair, a friendly expression, carrying 
fruit and draped in a light cloth concealing some flowers or grain since this 
wind caused a refreshing rain particularly beneficial to farmers 1 . Apeliotes 
name was chosen because it seems to be relevant since New Zealand is located 
geographically in the South East of the world, is largely involved in farming, 
and also a weather and flight aspect is implied. 

Apeliotes is a lightweight, low power magnetometer based unmanned 
aerial vehicle (UAV) flight control system. Apeliotes is a flight control sys- 
tem capable of flying a wide variety of unmanned model planes. To fly an 
aircraft must know its orientation. An attitude solution is vital for flight 
but can often be discounted by humans because they think that they know 
which way up is, but they don't. Not without looking that is. The inner 
ear canal will indicate which way is up for a short time. But if they close 
their eyes and turn around for some time, The person will soon loose track 
of the orientation of the world. That is until they open their eyes again. The 
horizon tells which way is up, while the floor and other objects tell which 

definition taken from Wikipedia online encyclopedia, written by Matthew Chave, 2005 
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Figure 1.1: The GWS E-Starter airframe 



direction the movement is in. 

The attitude and position of a model aircraft comprise a state vector with 
six degrees of freedom. Apeliotes can incorporate magnetometers, accelerom- 
eters, gyros, pressure sensors and the Global positioning system (GPS) to 
calculate accurate position and attitude information. 

A processor is used [2] to collect data from the sensors and calculate the 
current attitude and heading. This processor can then be used to control 
standard model R/C components such as servos and a speed controller to 
modify the state of the aircraft. This project is focused on the flight control 
system rather than the development of an airframe. For flight testing a GWS 
E-Starter was used, which is a standard low-cost electric model airframe (see 
Figure 1.1). 

1.1 Overview 

This report is formatted so that it takes the reader through the development 
process which Apeliotes went through. The initial design decisions, the cir- 
cuit boards which were made, and then the required mathematics for the 
control system which could then be implemented. The reading is intended 
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to be pitched toward a student who is beginning research in this topic. This 
write-up attempts to maintain a functional level of understanding without 
diving to deep into problems such as the difficulties involved in implement- 
ing a fixed point math library etc. Apeliotes is interchangeably referred to 
throughout the report as: the autopilot, Apeliotes, UAV, and autonomous 
control. 
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Chapter 2 

Design Considerations 



The development choices for the electronics of the flight control system are 
outlined in this chapter. The block diagram in Figure 2.1 shows the systems 
overview. 




Main Board Servos 



Figure 2.1: Hardware block diagram for UAV 



2.1 Sensors 

The selection of the sensors required was influenced by factors including 
price, availability, linearity, noise immunity, power consumption and weight. 
Figure 2.2 shows a photo of the sensors selected. Each of the sensors purpose 
is stated in this section. 
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Figure 2.2: Sensors from left to right: GPS antenna, GPS module, magne- 
tometers and pressure sensor, accelerometers, Gyros. 

2.1.1 Attitude 

The most important and elusive information needed to control an aircraft is 
the current attitude. The required attitude measurements are roll 9, pitch 
(f) and heading ip (also called yaw for simplicity although this is slightly in- 
correct). Inertial state inputs are typically integrated with respect to time 
giving the required attitude angles. This integration however is not reliable 
over long periods of time, typically larger than 30 seconds, due to the sensor 
drift and must be compensated and calibrated using other more reliable but 
slower reacting, or mathematically intensive sensors. Often the calibration 
for the inertial measurement unit (IMU) is achieved on a long time scale us- 
ing other sensors such as the global positioning system (GPS), which has an 
update rate of 1Hz, sun sensing, star alignment, horizon sensing, accelerome- 
ters etc. This control system uses a three axis magnetometer and the worlds 
12th order geo-magnetic model (which takes in date, altitude and geographic 
position). The model outputs a three axis magnetic field intensity, from 
which the direction vector of the local magnetic field can be established. Us- 
ing this vector as a reference the magnetometers can measure the current 
magnetic field vector relative to the plane and calculate a quaternion rota- 
tion required to map the measured vector onto the reference vector (Chapter 
4 describes this process). This magnetometer method will produce a result 
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Figure 2.3: Rotations about the magnetic field vector produce the same 
readings [6] 

for the attitude where the aircraft must be somewhere on the rotation about 
this calculated result graphically shown in figure 2.3. So we require the 
knowledge of one more rotation angle, for a complete solution. The airframe 
used has a large amount of dihedral and this project does not use ailerons 
so the roll can be assumed to be zero (see Section 5.6). So there will only 
be one position about the rotation which gives zero roll. Thus reducing the 
output requirements to just pitch and yaw. Which is discussed in Section 
5.6. 

2.1.2 Inertial State 

"The innate force of matter [inertia], is a power of resisting, 
by which every body, as much as in it lies, endeavors to persevere 
in its present state, whether it be of rest, or of moving uniformly 
forward in a straight line." [11] 

As quoted Newton's first law says that a body in motion will remain in 
motion unless acted upon by an external force. Inertial navigation is with 
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reference to coordinate frames. It is important therefore, to reference to an 
inertial (non-accelerating) frame. In aviation sensors are used to measure 
the rate of changes in position along and about a reference frames axis such 
as the body fixed axis of a plane. These measurements are then translated 
into an inertial reference frame where measurements are relative to the center 
of mass of the earth, the spin axis represents the z-axis, the x-axis can be 
measured with reference to star systems or where the sun crosses the equator 
during the equinox, then the y-axis is given according to the right hand triad 
[18]. For aviation navigation purposes a local level inertial reference frame 
is used where the axis are defined as NED or North East Down as positive 
directions and rotations according to the right hand rule [18] . 

Measurements are taken in the reference frame of the aircraft, called the 
Body frame since the sensors are carried on-board this approach is called 
the " strap down" method of inertial navigation. Measuring the inertial state 
of the aircraft's six degrees of freedom normally involves six inertial mea- 
surements including roll, pitch, yaw and translations along each of these 
directions, that is, accelerations and velocities along the three axes. Nor- 
mally detected directly using rate gyros and accelerometers, it is necessary 
to use three of each requiring six more sensors to directly calculate the an- 
gular rates and the components of acceleration. Instead, sampling the three 
magnetometer axis at regular time intervals using a timed interrupt routine 
and calculating the rates of change is a more lightweight, cheaper, lower 
power option. This is also possibly a more reliable method, due to gyros and 
accelerometers being susceptible to drift. 

2.1.3 Coordinated control 

In large aircraft the coordination of a turn is indicated by an inclinometer. 
The inclinometer is a glass tube, filled with kerosene, with a free rolling 
steel ball (an example is shown in Figure 2.4). The tube is C-shaped; its 
center is the lowest point closest to the floor, each end being higher. During 
coordinated flight the ball will sit in the center of the tube, said to be centered 
[5]. The ball is used to indicate to the pilot if the required amount of rudder 
input is being used, during maneuvers such as rolls and turns. 

While performing a roll maneuver if the ball deflects into the roll, then 
the rudder input is insufficient. Whereas if it deflects opposite the direction 
of the roll, the rudder input is excessive. 

While performing a turn the ball is used to indicate if the rate of turn is 
correct for the current bank angle. If the ball deflects toward the direction of 
the turn, the aircraft is said to be in a slip, more inside (or 'bottom') rudder 
should be applied. If however the ball deflects toward the outside of the turn, 
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Figure 2.4: Aircraft inclinometer [14] 



the aircraft is said to be in a skid, and more outside (or 'top') rudder should 
be applied. 

The ball is a single axis accelerometer orientated along the y-axis of the 
aircraft (along the wings). Accelerometers can be implemented electronically 
using Micro-Electro-Mechanical Systems (MEMS) devices such as the Ana- 
log devices ADXL302. More accelerometers could be used to measure the 
loading along the three axis so that the aircraft is flown within the abilities 
of the airframe, gyros could also be used to measure rotation rates so that 
a complete closed-loop control system can be implemented for maneuvering 
the control surfaces (see Section 5.4.1). 

2.1.4 Airspeed 

Forward airspeed is a critical component in the flight of an aircraft. The con- 
trol system must fly the aircraft within the envelope of the capabilities of the 
airframe. Which requires knowledge of the aircrafts stall speed and therefore 
knowing the aircrafts current airspeed is necessary. Several techniques where 
considered to sense air speed, which included: 

• Heating two transistors to a constant temperature. One in the airflow 
the other not, and then calculating the difference in current required to 
maintain the temperature in each of the devices. This effect however 
requires complex calibrations and is potentially not very linear in the 
event of turbulent flows. This stands out as the cheapest solution. 
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• Propeller rotating in the airflow. The rate would be either sensed 
by current produced, or by counting the rotations using some kind 
of opto-couple or Hall Effect sensor. While not specifically part of the 
requirement of this project this technique will cause some inefficiency, 
is bulky and is the most mechanical of the solutions. 

• Pitot tube. This uses a differential pressure sensor to take advantage of 
Bernoullis principle to measure the difference in pressure between static 
and dynamic air pressure relative to the aircraft. This solution uses the 
most expensive components, however it extends to a large range of air 
velocities, requires very little drag, is not too difficult to calibrate and 
is the industry standard. For these reasons this technique was chosen. 

2.1.5 Position Measurement 

Position and average velocity measurements are determined using the Global 
Positioning System (GPS). The device used is a Trimble Lassen SQ GPS 
module. The velocity vector is the result of the aircrafts current heading 
(attitude) with environmental and airframe effects all relative to the earth. 
These effects could include, cross wind (see section 5.5.5 for cross wind com- 
pensation), or inaccuracies in the calibration for the attitude (see section 5.1.3 
for calibrating the sensors) etc. The Trimble Lassen SQ GPS module uses 
the LI frequency 1575.42 MHz, the C/A code, and continuous tracking of 8 
satellites, with 32 correlations to achieve the parameters shown in table 2.1: 

2.1.6 Battery health and current consumption 

The plane converts energy from the stored chemical potential of the bat- 
teries or fuel into kinetic (velocity) and gravitational potential (altitude). 
This energy consumption and reserve must be monitored so that the con- 
trol system can decide what to do given the current state of these energy 
reserves. The battery health and consumption is monitored using two of the 
ARM7TDMI-S CPU controller modules analog inputs. For battery health 
one input monitors a calibrated resistor pair which drops the voltage from the 
9V of a healthy battery to 3V so that it is within a measurable range. Cur- 
rent consumption is monitored by passing the total supply current through a 
high power 0.003 Ohm resistor. The resulting voltage is then amplified using 
an AD8130 differential amplifier using suitably selected resistors so that the 
output voltage ranges from 0 to 3V for input to the ARM7TDMI-S CPU 
controller module's analog to digital converters. 
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Power consumption 


U0mW@3.3V 


Update rates 


TSIP@lHz; NMEA@1HZ; TAIP@IHZ 


Horizontal 


< 6 meters (50%), < 9 meters (90%) 


Altitude 


< 11 meters (50%), < 18 meters (90%) 


Velocity 


0.06 m/sec 


PPS 


95 nanoseconds 


Reacquisition 


< 2 sec. (90%) 


Hot start 


< 14 sec (50%), < 18sec (90%) 


Warm start 


< 38 sec (50%), < 45sec (90%) 


Cold start 


< 90 sec (50%), < 170 sec (90%) 


Dynamics 


Acceleration : Ag (39.2m/sec2) 


Motional jerk 


20 m/sec 3 


Operational Limits 


Altitude < 18000m 
or velocity < 515 m/s 
(COCOM limit) 

Either limit may be exceeded but not both 


Connectors 


Antenna -RF Low profile coaxial connector 
1 serial port (transmit /receive) 


Operating Temperature 


-40C < T < +85C 


Vibration 


0.008 g 2 /Hz @ 5Hz to 20Hz 
0.05 g 2 /Hz @ 20Hz to 100Hz 






± 3 dB/octave @100 Hz to900 Hz 


Operating Humidity 


5% to 95% R.H. non-condensing @ +60C 


Weight 


Approximately 5.7 grams (0.2 ounce) 



Table 2.1: Trimble Lassen SQ parameters (from [8]) 



2.2 Support Hardware 

This section describes the separate integrated circuit's (IC's) used to interface 
to the sensors, servos, radio receiver, etc to the processor, including the 
selection of this processor. 

2.2.1 Analog input 

The sensors supported by the control system require 13 analog inputs, the 
processor core contains 4 analog to digital converters (ADC), of which two 
are unavailable due to their other functionality (see [15] for the pin-out in- 
formation of the processor). The large number of analog inputs could be 
multiplexed into the available pins but this would require routing past the 
noisy digital signals on the main board and would still require at least one 
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extra integrated circuit (IC). To resolve this problem a dedicated ADC IC 
was selected. The ADC chosen was the MAXIM, MAX1231. This IC is 
capable of sampling 16 inputs at a total of 300ksps. The internal reference 
temperature input would mean that an easily accessible analog input would 
be readily availably when it was time for programming, and also provides 
a means for main board temperature measurement. The chosen ADC has 
an accuracy of 12-bits which gives a resolution of approximately 3V / 4095 
which is 0.733mV. For the attitude reference from the magnetometers this 
would mean an ADC resolution of approximately 0.9 degrees which adequate. 
The digital interface provided uses the SPI (Serial Peripheral Interface) pro- 
tocol which will use six pins to talk to the processor. The SPI interface is 
desirable because it is implemented in hardware on the chosen processor, so 
therefore transparent to the users processor tasks. For further functionality 
of the MAX1231 see [9]. 

2.2.2 Mode selection UAV/RPV 

The autopilot must have a way of being disabled so that a human pilot can 
take over the control of the aircraft. This is useful for testing small parts of 
the control system incrementally (see chapter 7 for testing). The UAV/RPV 
(Unmanned Air Vehicle/Remotely Piloted Vehicle) selector is attached to the 
fourth channel of the RC transmitter. The selection works using a data latch 
setup so that as the rising edge of the signal from the receiver is detected this 
starts a timer (MC4013B for the timer and D latch). This timer counts for 
a time which can be modified by rotating the variable resistor on the main 
board. Typically the length of this timer would be 1.5ms which represents 
the transmitters stick in the center position. When the time is reached the 
pulse level from the receiver is latched. If the signal is still high then the Q 
value output from the data latch will be high and so the octal buffer/line 
driver (74LS541DW) which has its chip select attached to this line will be 
on, while the octal buffer/line driver which has its enable line attached to 
the Q bar output will be disabled. The state of the system can immediately 
be seen by the red indication LED (Light Emitting Diode) located next to 
the power switch. The circuit is shown in Figure 2.5 

2.2.3 (De)Multiplexing the controller to servo output 

The servos each require a pulse of varying width about 1.5ms at least every 
30ms. The control system currently has 8 servo outputs which it talks to 
using a Johnson counter (MC14017BD). The Johnson counter uses two inputs 
CLK and RESET and is capable of driving 10 outputs, it can also be daisy 
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Figure 2.5: UAV/RPV switch 



chained so that this number can be what ever is required. For this system 8 
servo outputs should be more than enough, 8 was chosen because there where 
8 line driver input and outputs and also the 8 outputs could be, for example, 
Rudder, aileron, elevator, throttle, gear, flaps this would use all the outputs 
depending in the layout selected and the level of control required. The control 
system uses a low overhead interrupt routine so that the processor is not 
always busy counting in a delay loop. The interrupt routine is triggered 
by one of the processors timers which is reset then loaded with four values 
to count to. Upon reaching each value the interrupt routine is triggered 
which outputs a short pulse to the Johnson counter. Once the internal timer 
matches the fourth value the next four servos values are loaded into the timer 
match registers and the timer is reset. When the timer matches the fourth 
value this time around the Johnson counter is reset along with the timer and 
the new first four servo values are loaded into the timers match registers, 
repeating the process approximately every 16ms. The Johnson counter takes 
in this serial data stream and changes the state of two neighboring output 
pins simultaneously on each rising edge of the CLK signal. Once a reset is 
issued the Johnson counter drives all of its outputs to low except for output 
zero which is driven high. Now when the reset line is set back low and a CLK 
rising edge is detected the zeroth output will change state to low and the first 
output will become high. So the controller should send a pulse which returns 
to low so that the controller is ready to issue the triggering signal as soon as 
the time value is correct. The timing diagram for this process is shown in 
Figure 2.6 
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Figure 2.6: Timing Diagram for the Johnson counter from [16] 
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2.3 The processor 



The system designed uses one processor to compute everything required for 
autonomous flight. This includes; attitude and navigation solutions, software 
filtering, aircraft control and telemetry so a powerful processor is required. 
The processor needs also to be small, light weight and energy efficient so that 
no bulky cooling solutions are required and battery longevity is reasonable. 

To fulfill these requirements an ARM7TDMI-S high end micro-controller 
module [2] was selected. This processor has the following specifications: 

• Phillips LPC2129 ARM7-TDMI micro-controller. 

• Better than lmW / MIPS (milliwatts per million instructions per sec- 
ond). 

• ARM7 core from Advanced RISC Machines. 

• 32-bit RISC (Reduced Instruction Set Computer). 

• Board size of 51mm x 29mm. 

• Low cost. 

• 3.6 to 5.5V supply voltage, with on-board 1.8V and 3.3V supplies, the 
3.3V supply being capable of supplying 300mA of current. 

• 14.7456 MHz oscillator clock with a 1, 2, 3, or 4 times multiplier which is 
dynamically selectable for power on demand (such as could be required 
upon reaching a waypoint). 

• 60 MHz maximum clock rate, with 1 instruction per cycle. 

• Socket-able via two dual row 40 pin 1.27mm headers 
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Chapter 3 

PCB Design and Layout 



The design constraints for the circuit board where to minimize crosstalk 
where necessary, minimize size, minimize the time spent to get something to 
work and layout the board so that it is balanced and the components can be 
easily plugged in while still making the necessary components accessible to 
the operator. 

3.1 Main Board 

A photo of the fully assembled main PCB is shown in Figure 3.1 with the 
board layout detailed in Figure 3.2. The board is laid out so that the analog 
data is as far away as possible from the digital data with a ground plane 
running underneath. The servo outputs are grouped together on the side 
of the board, with the ARM processor module in the center. The power 
input, switches and sensing are at the end opposite the analog to digital 
converter. The 8 header switches are accessible for reversing the direction 
of the individual servo outputs. The board is oblong shaped so that it will 
easily fit into most model sized aircrafts fuselages. 

3.2 Sensors 

3.2.1 Triaxial magnetometer 

The magnetometer board has the sensing, filtering and amplification stages 
for the magnetic field implemented. The magnetic field sensors used are 
Honeywell magneto-resistive devices, internally configured as a Wheatstone 
bridge. These small and low cost sensors are capable of sensing magnetic 
fields as low as 30figauss with a field range up to ±6gauss, where the earths 
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Figure 3.1: Fully assembled main board, the 40 pin connectors are for the 
ARM module 




Figure 3.2: Eagle Board design picture 



24 



field is approximately 0.5gauss. The sensors used in this circuit are the 
HMC1022 2-axes sensor, and the HMC1021Z single axes sensor. 

This sensor board is connected to the main board via an 8 pin cable. The 
cable carries the supply voltages for the on-board circuitry, the reset line for 
the magnetometers and the amplified analog signals for input to the main 
board. This board is designed to be very small so that it may be placed 
in the tail of the aircraft away from the engine noise, without affecting the 
weight distribution of a small aircraft too much. 

Figure 3.3 shows the design of the board at what should be the actual size, 
and Figure 3.4 shows a photo of the assembled board. The magnetometer 
sensors consume approximately 50mW each, for a combined 150mW across 
the 3-axes. 




Figure 3.3: 3 axis magnetic field board design layout, measuring 5cm by 2cm 




Figure 3.4: 3 axis magnetic field sensor board 
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Chapter 4 



Magnetometer Attitude 
Determination 

Determining attitude information from a magnetic field vector can be achieved 
by two main methods. Some micro and nano satellites use magnetometers 
to calculate the rate of change of the magnetic field vector about the three 
axes. The rate of change is then integrated over time through an extended 
Kalman filter (for more information on Kalman filters see [3]) to give an esti- 
mation of the crafts current attitude. This method does not require any prior 
mapping or modeling of the local magnetic field vector since only rates of 
change are measured. The second method, the method which is used here for 
Apeliotes, takes advantage of the earths geomagnetic field being well mapped 
and modeled. This second method uses a local earth referenced calculated 
magnetic field to compare with the body referenced measured field to deter- 
mine the current attitude of the aircraft. This chapter describes how this 
second method is achieved. 

4.1 Local reference magnetic field calculation 

Accurate models of the earths magnetic field have become extremely accurate 
due to the extensive work carried out by The International Association of 
Geomagnetism and Aeronomy (IAGA), the United States National Geophys- 
ical Data Center (NGDC), and the British Geophysical Survey (BGS) [10]. 
Their interests for modeling and collecting data on the magnetic field include 
study of the earths deep interior, crust, ionosphere, and magnetosphere. 

The model developed to date uses a thirteen degree and order spherical 
harmonic approximation as shown by Gauss in 1839, with coordinate trans- 
formations as outlined by the World Geodetic System 1984 (WGS84) [17]. 
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The parameters for the WGS84 are 

a = 6378.137 km and 
b = 6356.752 km 

To adjust for the time varying components the mathematical models of 
the Earth's main field track its annual rate of change (secular variation). In 
source-free regions at the Earth's surface and above, the main field, with 
sources internal to the Earth, is the negative gradient of a scalar potential V 
which can be represented by a truncated series expansion: 



where r, 9, A are geocentric coordinates (r is the distance from the center 
of the Earth, 9 is the co-latitude, i.e. 90 - latitude, and A is the longitude), R 
is a reference radius (6371.2 km). g™(t) and h™(t) are the coefficients at time 
t and P™(9) are the Schmidt semi- normalized associated Legendre functions 
of degree n and order m. The maximum spherical harmonic degree of the 
expansion is N. The coefficients are functions of time which are assumed to 
have linear change over five-year intervals, since the magnetic field is not 
yet fully understood these coefficients are updated every five years. For the 
upcoming five-year epoch, the rate of change is given by predictive secular 
variation coefficients (for further reading on the modeling process see [4, 13]). 

The International Geomagnetic Reference Field's (IGRF) magnetic field 
values currently have a precision of O.lnano-Tesla (nT). This is approxi- 
mately one hundred thousandth the size of the earths magnetic field which 
is about lOOOOnT. The IGRF model coefficients and source code (available 
in C and FORTRAN) are available freely for download on the internet from 
the IAGA's website. 

The model took as its inputs: 

• Geocentric or Corrected GeoMagnetic Latitude and Longitude 

• Altitude above the earths surface at 6371.2 km with a 40,000 km ceiling 

• Year for Definitive/International Geomagnetic Reference Field (DGRF/IGRF) 
Giving the following outputs: 

• Total magnetic field intensity in nT 




[10] 
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Figure 4.1: World map of the magnetic declination as of 2005, in degrees 
with contour interval of 2 degrees [12] 

• Horizontal magnetic field intensity in nT 

• North Magnetic field intensity in nT 

• East Magnetic field intensity in nT 

• Down (vertical) Magnetic field intensity in nT 

• Magnetic field inclination in degrees 

• Magnetic field declination in degrees (see Figure 4.1) 

4.2 Body referenced magnetic field given de- 
sired attitude 

Now that the co-ordinates of the magnetic field in the earth referenced frame 
are known B e , it must be combined with the current desired attitude. This 
produces a vector which should be equal to that measured by the magnetic 
field sensors when the aircraft is flying with the desired attitude. The trans- 
formation between the earth referenced field and the body referenced field 
is done using quaternions (see Appendix A). Quaternions are used because 
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they are a computationally efficient and logical way of handling rotations. 
The Quaternion operates by producing a vector which when rotated around 
by a determined amount will map a second vector onto a third vector. This 
single rotation removes the ambiguity which is present in Euler angles due 
to them being non-commutative (see [7]). 

The Euler attitudes 9, <p and tp are combined into quaternions as follows: 



, 9 9 
q e = {cos-, sin-, 0,0) 

q<t> = (cos-, 0, sin-, 0) 

ip if) 
q<p = {cos-, 0,0, sin-) 

The quaternions are multiplied together to produce total rotation, 



See Appendix A.l for details on quaternion multiplication. The final rotation 
quaternion is then, 



q T 



( 



cos^cos^cos^ 



— sin^sin^sm 2 



t \ 



i * sin^sin^cos^ + i * cos^sin^cos^ 

8-6 ib ■ ■ 6 d> ■ ib 

j * cos^sm^cos^ — j * sin^cos^sm^ 

^ k * cos^cos^sin^ + k * sin^sin^cos^ j 



Applying this rotation quaternion to the reference field vector B e rotates the 
zero attitude vector to the desired attitudes body referenced vector, 



—desired 



QrRzero attitudelT* 



Where qT is a unit vector q T * {w,i,j,k) = qr{u!, —i, —j, —k) so that the 
imaginary parts are just reversed in sign. Then we get the final result (see 
Appendix B.l for combined example code) 

x_field = 

Ta*Ta*trueN_f ield.x + 2*Tc*Ta*trueN_f ield.z - 2*Td*Ta*trueN_f ield.y + 
Tb*Tb*trueN_f ield.x + 2*Tc*Tb*trueN_f ield.y + 2*Td*Tb*trueN_f ield.z - 
T3*T3*trueN_f ield.x - Tc*Tc*trueN_f ield.x; 



y_field = 

2*Tb*Tc*trueN_f ield.x + Tc*Tc*trueN_f ield.y + 2*Td*Tc*trueN_f ield.z + 
2*Ta*Td*trueN_f ield.x - Td*Td*trueN_f ield.y + Ta*Ta*trueN_f ield.y - 
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2*Tb*Ta*trueN_f ield.z - Tb*Tb*trueN_f ield.y ; 
z_field = 

2*Tb*Td*trueN_f ield.x + 2*Tc*Td*trueN_f ield.y + Td*Td*trueN_f ield.z - 
2*Ta*Tc*trueN_f ield.x - Tc*Tc*trueN_f ield.z + 2*Ta*Tb*trueN_f ield.y - 
Tb*Tb*trueN_f ield.z + Ta*Ta*trueN_f ield.z; 

where Ta, Tb, Tc and Td denote the four terms of the defined rotation 
quaternion. trueN jield.axis denotes the field measured along the denoted 
axis when the aircraft has zero attitude (i.e zero roll, pitch and heading). 
Finally axis_field denotes the body referenced magnetic fields for a given 
attitude as defined by 6, <fi and ip earlier. 

4.3 Calculating rotation to achieve desired 
attitude 

Now that a reference vector has been established, which is what should be 
measured if the plane has the desired attitude, the current magnetic field 
must be measured. This is done using the 3-axis magnetometer described in 
3.2.1. 

The vector is measured with reference to the aircraft (Body frame) so any 
rotations about this measured vector produce exactly the same measurement, 
Figure 2.3 helps visualize this. Therefore we can only calculate two rotations 
without using any other sensors such as more magnetometers or horizon 
sensors. However if we can assume that there is zero roll (valid for the current 
airframe when not turning, see section 5.6) so that one of the three rotations 
is removed, then the heading and pitch rotations can be resolved. There are 
two important exceptions here, the first is that at the poles (magnetic poles) 
the heading is undefined. The second case is that at the magnetic equator 
when flying along this equator the pitch is undefined due to a zero intensity 
in the X-Z plane, the heading also measures the same either in the east or 
west direction (one way is flying upside down). These are the only obvious 
failures. All other locations and attitudes do not have these problems because 
the known roll measurement means that only one solution is available. 

So if the body referenced observed magnetic field is measured to be 
M x , My, M z then the rotation Q w , Q i , Qj and Qk required to map to the 
desired field D x , D y and D z is 

Q = Q M (0, M x , M y , M z ) x Q D (0, D x , D y , D z ) 

The final result for the Quaternion rotation which would map the current 
aircrafts attitude to the desired aircrafts attitude turns out to be. 
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qO = -trueN_f ield.x*observed_f ield.x - trueN_f ield. y*observed_f ield 
- trueN_f ield . z*observed_f ield . z ; 

ql = trueN_f ield. y*observed_f ield. z - trueN_f ield. z*observed_f ield. 
q2 = -trueN_f ield. x*observed_f ield. z + trueN_f ield. z*observed_f ield 
q3 = trueN_f ield. x*observed_f ield. y - trueN_f ield. y*observed_f ield. 

Excerpts from the source code can be found in Appendix B.3 with multipli 
cation from Appendix A.l. 
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Chapter 5 

Flight Control System 



The flight control system uses the sensor information as feedback, performing 
attitude estimation to set the control surfaces of the aircraft as output to 
change the orientation of the aircraft to the desired attitude (Figure 5.1). 
This chapter describes how this process is performed. 

5.1 Sensor measurement 
5.1.1 Conditioning 

Conditioning of the signals which come from the sensors is necessary since 
they are often noisy and small in amplitude. The magnetometers are placed 
as far away as possible from the engine and main board. Ideally this long 
range link should be digital so that it is more immune to noise however this 
is not yet implemented. The analog signals can be filtered using a simple 
second order low pass Butterworth filter such as that in 5.2. 
With the component values of : 

• Rl = 50 Q, 

• R2 = 50 Q, 

• LI = 0.375075 H 

• CI = 0.00015003 F 

To produce a 50 VL input impedance, and a frequency profile as shown in 
5.3 which has a reasonable cutoff for frequencies above 20 Hz. Twenty Hertz 
was chosen because some model aircraft are capable of doing a complete 
revolution in fractions of a second and so a reasonable bandwidth is necessary, 
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Figure 5.1: Implementation of the control system 




Figure 5.2: Second order low pass butterworth filter 
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Figure 5.3: Spice simulated filter, red represents amplitude, blue phase, with 
frequency horizontal. 

but we are not really interested in oscillations above 20 Hz as the aircraft 
which would be chosen will have a reasonable amount of inherent stability. 

The signals are then amplified using a non-inverting amplifier (5. 4). The 
Non-Inverting Amplifier multiplies the input voltage, Vi n by the desired pos- 
itive gain, and subtracts a voltage proportional to the applied reference volt- 
age: 

V out = V m x(l + Rf/Rg) - V ref x(R f /R g ) 

Where the feedback capacitor Cf rolls off the gain at frequencies above 
Y~ x RfCf, to attenuate high-frequency noise. 

The following values were chosen for the components of the magnetometer 
amplifiers: 

• Al = LMV324 

• C f = 0.002400 uF 

• R f = 196000 n 

• R g = 2000 tt 
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Figure 5.4: Non-Inverting amplifier schematic 

5.1.2 Sampling 

The analog signal is sampled by an analog to digital converter (described 
in Section 2.2.1). The data is sampled inside the ADC and averaged over 
32 samples. Currently this sampling process is called from within the code 
whenever readings are needed, but in the future should be done at a prede- 
termined time interval such as every 25ms using an interrupt routine. Then 
the signal can be mathematically operated on, such as integrated, because 
the time intervals of the samples are known. Sampling at 40 Hz gives a signal 
frequency detection of up to the pre-determined 20 Hz. 

5.1.3 Calibration 

The calibration process is currently required because many of the compo- 
nents are not perfect. For example the magnetometers each respond to the 
magnetic field with some varying degree of accuracy and offset, the ampli- 
fiers do not all have the same gain and the signals are attenuated slightly 
differently as they propagate through to the ADC. So the signals mush each 
have an offset value and a gain associated with them. These are calculated 
using the following process: 

• Set the flag in the UAV.cpp code for calibration output. This outputs 
serial data down the secondary RS232 port on the side of the main 
board. The format is: Xfield XfieldMIN XfieldMAX Yfield YfieldMIN 
YfieldMAX Zfield ZfieldMIN ZfieldMAX 

• The operator must then orientate the aircraft while monitoring the out- 
put values to try and maximize and minimize the appropriate results. 
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• Now the offsets and gains can be calculated using the following formulas 



max — mm 
Of j set = h mm 

„ Greatestfmax — min) 

Gam = 

max — mm 

• Now the operator must adjust the appropriate variables found in mag- 
netometer. h for the new values. 

The calibration is a once off operation which takes around 20 minutes. The 
values must be recalculated when relevant components of the board are mod- 
ified, or for the magnetometers, if the airframe is modified in such a was that 
the magnetic field would be effected. The values should be stored for future 
reference. 



5.2 Attitude detection 

Using the magnetic field and quaternion rotation method described in section 
4 the attitude of the plane can be deduced. The practical implementation 
is fairly simple. The magnetometers are mounted on a rigid mount near the 
tail of the airframe, so that all attitudes calculated are with reference to this 
mounting location. Using the magnetometer attitude method, the sensor 
does not need to be mounted close to any of the rotation axis of the plane 
since there is no coupling from the movement as there is for accelerometers 
and gyros. So the mounting location can be anywhere most convenient, 
preferably away from noise sources. Since the values which come from the 
sensors go through a significant amount of mathematical manipulation their 
values are modified so that they are close to unary. The attitude values are 
then stored in specific memory locations accessible by other parts of the code. 



5.3 Output 

The control system electronically outputs to the outside world using three 
types of communications; 2-wire serial data for the control surfaces, RS-232 
for the GPS and telemetry/debugging, and SPI 
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5.3.1 Servo output 



The servos each require a pulse of varying width about 1.5ms at least every 
30ms. The control system currently has 8 servo outputs which it talks to 
using the Johnson counter. The Johnson counter uses two inputs CLK and 
RESET. To output the 8 required signals the control system uses a low 
overhead interrupt routine so that the processor is not always busy counting 
in a delay loop. The interrupt routine is triggered by one of the processors 
timers which is reset then loaded with four values to count to. Upon reaching 
each value the interrupt routine is triggered which outputs a short pulse to 
the Johnson counter. Once the internal timer matches the fourth value the 
next four servos values are loaded into the timer match registers and the 
timer is reset. When the timer matches the fourth value this time around 
the Johnson counter is reset along with the timer and the new first four 
servo values are loaded into the timers match registers, repeating the process 
approximately every 16ms. 

5.3.2 Telemetry /Debugging output 

The control system is capable of outputting and inputting data using a fully 
functional serial RS232 port. The 9-pin plug on the side of the motherboard 
is used to plug into (also the GPS port on top of the processor can be used). 
This port is useful for outputting debugging information while developing 
the on-board code. While ground testing the control system the port can be 
used to output telemetry information. While in flight the port can be used 
to plug a data logger or a modem to capture the telemetry for display on the 
ground. 

5.3.3 SPI communication for ADC and miscellaneous 
on-board packages 

The main board uses an SPI protocol to communicate with the ADC and is 
available for any additional on board packages use via a five pin header on 
the main board. This is a high speed serial communications protocol capable 
of sending and receiving full-duplex data at a 40MHz clock rate. 

5.4 Simple first approach 

For this control system a first order proportional control is used. Apeliotes 
currently uses the first, P part, of a PID (proportional, integral, and deriva- 
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tive) algorithm. A PID algorithm takes a reference value measures an input 
and attempts to drive an output to the desired reference value. Meanwhile 
the input is continually manipulated by external influences, and the output 
of the PID controller. The P is implemented only at the moment to obtain 
the systems response to determine what needs to be improved by adding the 
I and D components. 

5.4.1 PID control 
Proportional 

The components of a PID algorithm are all driven by the difference between 
the process value and the reference value. This difference is the current error 
in the state of the aircraft. For a particular time step this error can be 
denoted e n . For the same time step, the process value is y n and the reference 
point r n . 

Cn r n y n 

The output value requested is called u n . Proportional control simply 
calculates u n based on the magnitude of the error e n by multiplying it by a 
constant, Kp. 

u p = K P x e n 

Only using this for the control algorithm will produce satisfactory results 
for an initial attempt. Proportion only control will work provided that when 
e n = 0 then u n — 0. So the aircraft needs to be trimmed correctly when only 
using this control. The trim is adjusted using a servo control function which 
is preset for the desired trim signal for each servo. 

Integral 

Where the aircraft is not perfectly trimmed and in most cases it wont be, 
since the amount of control surface deflection needed to drive the error to zero 
can vary with speed, a proportional only controller may stabilize out quickly, 
but probably to the wrong value. So to drive the error to zero the Integral 
component of the PID algorithm will be required. This is the area error under 
the process curve. If each time step of e n is known, then by multiplying this 
error by the time step dt an area is calculated which approximates the error 
under the curve for this time step. If the areas are summed over time the 
result is a reasonable approximation of the area under the curve. 
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n 

Uj = Ki x e n x dt 

n=l 

So the longer time passes that the target value is not achieved, the larger 
that the sum of the difference becomes over time. If this sum is used to 
push the output value then the more time that passes that the target is 
not achieved, the more the system forces the output. So the integral can 
compensate for the trim error in the proportional component and the system 
will stabilize out with the desired value. 

derivative 

The derivative function gives the rate of change of the measured input. This 
rate of change gives how fast the system is converging (or not converging) 
on the target value. 

u d = K D x — 

So here the amount of output is proportional to the rate of change in the 
input. 

combining the three 

Now a more stable controller can be built capable of achieving the target value 
at an optimum rate for an accepted level of oscillation about this target. 

A proportional only controller is stable but can stabilize to the wrong 
value. An integral only controller will quickly hit the target value, but will 
overshoot, then overcompensate, oscillating about the target value. Whereas 
a derivative only controller would drive the output further in the direction 
of the changing error without knowing the target set point. The I and D 
components are unstable. 

The trick then is to combine these components together by summing 
them. The actual output is equal to what the P component says the output 
should be plus what the I component says the output should be plus what 
the D component says the output should be. 

U n = U p + Ui + U d 

While the math involved in a PID controller is quite simple to implement. 
The trick for creating a well behaved control system is tuning the relative 
weights K P , Ki and K D of each of their respective P, I and D components. 
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5.4.2 Attitude Control 



Roll, Pitch and Heading constitute the three degrees of rotation and the way 
in which they can be immediately modified is by using Aileron, Elevator 
and Rudder respectively. Since the quaternion rotation required to map the 
measured attitude to the commanded attitude is known then the control 
system has every thing it needs to know. Since it has already calculated: 

q = cos(-) + i(x * sin(—)) + j(y * sin(— )) + k(z * sin(—)) 
2 2 2 2 

Where x, y and z represent a unit vector, about which we are rotating. So 
since this is the axis of rotation then this is the error vector e n about which 
the plane should turn. If the vector is all in the z axis then the plane needs 
to do a yaw rotation using the rudder. If the vector is completely in the y 
axis then the plane needs to perform a rotation using its elevators. These 
rotations are combined to cover all the combinations of attitudes. The total 
amount of rotation required is given by the real term and the lengths of the 
respective axis. This measurement is used as an error magnitude e n , about 
the respective axis components and fed into the servo controller, a function 
called calc_control(). Which takes into account the airframes responsiveness 
about each axis by applying a preset gain value K as per section 5.4.1. This 
gain value is currently calculated by the operator for each airframe by a trail 
and error process. The GWS E-Starter's values can be seen in B.4. 

5.4.3 Simple turn 

A turn is currently commanded by rotating the reference field vector as per 
section 4.2. This causes an error between the measured vector and this 
reference vector and so a rotation is calculated. This rotation is then used 
as an error signal e n which feeds through as described above in section 5.4.1. 

5.5 Navigation 

In order to have the full functionality of an autonomous vehicle the aircraft 
must be capable of navigation. This section describes how navigation is 
intended to be done. However, although most of the coding has been done 
(for example see Section B.5). The navigation algorithms discussed here are 
not on the current version of the control system. 
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5.5.1 Measuring the current location 

To measure the current location a standard GPS OEM module is used as 
per section 2.1.5. This module is capable of measuring the aircrafts absolute 
position to within an error of 10m. So this should be taken in to consideration 
when setting criterion for the waypoints. 

5.5.2 Waypoints 

The waypoints are to be hand selected by the operator. These could be 
chosen from topographical mapping software and typed into a table which 
is uploaded onto the control system. The plane must be able to achieve the 
waypoints to within some accuracy so careful selection and planning must 
be used. 

5.5.3 Achieving way points 

A waypoint is to be determined as achieved if the aircraft is within an imag- 
inary sphere which surrounds the waypoint. This sphere could be elongated 
so that it resembles more of a cylinder so that only passing through the 
appropriate latitude and longitude would register the waypoint as achieved 
and request the new waypoint from the waypoint state machine. The plane 
should attempt to achieve the center of each predetermined waypoint. 

5.5.4 Calculating required attitude to the next way- 
point 

The heading calculation required to get from the aircrafts current position 
to the next waypoint using great circle navigation is shown in appendix B.5. 

This uses the latitude and longitude and altitude latl, lonl and altitudel 
of the desired waypoint and the latitude, longitude and altitude latl and lonl 
and altitudel of the aircrafts current position to determine a distance and 
heading to the next waypoint. Granted this is overkill for the current state 
of the project. 

5.5.5 Course corrections, adjusting for environmental 
effects 

Course corrections between the Heading (measured by the magnetometer), 
the direction the aircraft is facing, and the Bearing, the direction of the 
aircrafts velocity relative to the earth (comes from GPS) gives the error in 
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wind 




Figure 5.5: Wind can be calculated as the difference between heading and 
bearing 

flight path due to wind. This wind vector can be calculated as the difference 
of the two vectors. As in figure 5.5.5. 

5.6 Flight dynamics 
5.6.1 Roll 

The control system assumes that there is zero roll to be correct. This as- 
sumption is discussed here, and in the results section. This assumption is not 
true at all times, nor is it at all true for all airframes. So the effects which 
cause roll should be understood to know how to reduce the effect or to make 
sure that, and understand how the airframe will re-stabilize laterally (about 
the X axis). 

The pendulum effect 

A high wing configuration uses the pendulum effect to self-level the plane 
about the axis of roll. Of course this means that the lower the Center of 
Gravity (CG) is relative to the wing, the greater this roll moment will be, 
since the mass will provide a greater torque moment. While lowering the CG 
now appears to be beneficial the effect of a side gust of wind should be taken 
into account. If the Center of Pressure (CP) from the wind force is below 
the CG of the plane then the aircraft will roll to oppose the force. However 
if the CP is above the CG then the aircraft will roll to enforce the disturbing 
force. So the CG should be low to self level the wings but not too low that 
the airframe becomes unstable due to side wind forces. 
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Dihedral slip-roll coupling 

In the presence of dihedral, a disturbance of uncoordinated (side-to-side) 
airflow causes an aircraft to roll away from its normal position, the aircraft 
will side slip in the direction of the down-going wing. This creates a lateral 
airflow component along the length of the wing. The dihedral angle can be 
seen as presenting a positive angle of attack to this lateral flow on the lower 
wing and a lower angle of attack on the higher wing, therefore providing 
a restoring torque which is proportional to the dihedral angle and propor- 
tional to the amount of slip. This returns the aircraft to its normal attitude. 
Preferably, due to the assumptions, to zero roll, if trimmed correctly. Also 
the fuselage shadowing one wing most notable on high- wing aircraft produces 
slip-roll coupling, swept wings produce slip-roll coupling, and a tall rudder 
that sticks up above the roll axis produces slip-roll coupling. The slip-roll 
coupling combines with the long-tail slip effect where there is a long tail 
present to give the airplane roll-wise stability. 

Differential Wingtip Speed, Over Banking 

When an aircraft is in a coordinated turn the outside wingtip follows a path 
of length 2 7r R, while the inside wingtip follows a smaller radius of 2 7r r. 
The outside wingtip must travel a greater distance in the same amount of 
time; therefore it has to move faster. 

Since the lift generated by an airfoil depends on the square of the airspeed, 
the outside wing produces more lift. This causes the aircraft turning to tend 
to bank more and more into the turn, called over banking. The tighter the 
turn, the greater the velocity difference becomes which can lead to a spiral 
dive. To reduce over banking the ailerons should be deflected against the 
turn. 

This effects torque depends on the ratio of the wingspan to the radius of 
turn. Short wings, high airspeed, and shallow bank angle, have less effect 
than long wings, low airspeeds and steep turns. 

Propeller Drag Rolling Moment 

The engine also contributes to the roll- wise torque budget. The propeller does 
not throw the air straight back. There is rotational drag on the propeller 
blades. Newtons law of action and reaction [11] says that if the prop throws 
the air down clockwise, it tends to make the airplane roll anticlockwise, as 
viewed from behind. 

Some of the rotating air hits the top of the right and the bottom of the 
left wing and horizontal stabilizer. This reduces the amount of roll, but 



43 



not completely. Using Newtons law again, any air which escapes while still 
rotating clockwise, must torque the airplane anti-clockwise. 

To counteract this effect a corresponding amount of air must be throw 
down on the opposing the rotation. This can be achieved by setting the 
left wing at a slightly lower angle of incidence than the right wing, Called 
asymmetric angle of incidence. The asymmetry should be in the part of 
the wing that flies in the prop-wash, so that the effect increases as engine 
power increases. Also the ailerons are used to counter the prop drag rolling 
moment. 

Engine Inertia 

Torque equals the rotational inertia times the rotational acceleration as from 
Newton [11]. So if engine revs are increasing or decreasing, a torque will be 
produced. 

Due to conservation of momentum if a clockwise rotational momentum 
is given to an object, then a counter-clockwise rotational momentum was 
imparted on something else. So if the engine speed is changing, the airplane 
will tend to roll if the rotational momentum is not counteracted. This is 
worst in the case where the engine and propeller spin in the same direction. 
So using a gearbox would be preferable. Also if you change the orientation 
of the plane of rotation gyroscopic precession will causes moments about the 
other axis. 

Discounting Roll 

All the effects mentioned can be combined so that a high-wing, swept-wing 
airplane with lots of dihedral ,a really high tail and a counter rotating prop 
would probably give more roll stability than is necessary. 

A positively stable aircraft when disturbed from its trimmed flight state, 
will commence an initial movement back toward the trimmed flight state, but 
probably over-run it. If the oscillations are damped the aircraft is said to 
have dynamic stability. An aircraft which initially makes a movement back 
toward the normal flight state is called statically stable. The magnitude of 
the oscillation and the time for the oscillations to damp out gives the aircrafts 
response rate to instability. Unfortunately a statically stable aircraft can be 
dynamically unstable in that plane i.e. the oscillations do not damp out. 

With slip-roll coupling a relatively normal turn can be performed using 
just the rudder. If you gently press on the right rudder, you will cause a skid 
that will eventually produce a bank to the right. If you hold a constant rudder 
deflection, the turning force will be proportional to the rudder deflection, 



44 



whereas the bank will keep getting larger and larger because of the slip-roll 
coupling. 

If, for some reason, one wing goes a bit lower than the other so that some 
roll is incurred the airplane will begin to turn. If the turn were perfectly 
coordinated, the airplane would continue to turn around. Fortunately, this 
type of turn will be a slipping turn. The small amount of slip, acting through 
the slip-roll coupling, will tend to roll the airplane back to level flight about 
the roll axis. Provided that the un-commanded bank does not approach 
the limit where the over banking tendency tends to takes over, the aircraft 
will remain stable in roll in the long term, provided the airframe is chosen 
appropriately. 
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Chapter 6 
Airframe 



The control system described in previous chapters can be used for any servo- 
controlled airframe. We chose the Grand Wing Servo-tech (GWS), E-Starter 
as the airframe for the first flight tests. A photo of the completed airframe 
is shown in Figure 1.1. 

The Specifications for the test craft can be seen in table 6.1. 

The airframe was chosen on the following parameters, according to the 
effects outlined in section 5.6.1: 

• Large amount of dihedral. 

• High tail plane. 

• Long tail. 

• Gear boxed engine and propeller for counter rotation. 

• Light weight so that it can be electric powered removing the setup time 
required of petrol motors. This also renders the plane fairly harmless. 

• Cheap, the airframe cost less than fifty NZ dollars. 

• Low airspeed, so that visual range is maintained for a long amount of 
time, necessary for viewing the performance of the autopilot. 

• maneuverability, the plane is capable of being put into extreme atti- 
tudes so that the autopilots recovery from such states can be assessed. 
Also if the auto pilot failed the remote pilot could recover control easily. 

• The airframe is an ARF (Almost Ready to Fly) kit so no time would 
be spent on design. Also the kit can be assembled easily in a day. 
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• Polystyrene airframe meant that any damage could be fixed with some 
glue at the test site so that the plane could be ready quickly for another 
flight. 



A /f ^ 1m 

lvi&Ke 


lorranci wing servo-tecn ^LrVVaj 


Model 


E- Starter 


1 r\T o 1 tt to l re nT 

±oiai weigiii 


u.oyo ivg 


Length 


0.725 m 


Wingspan 


0.960 m 


Wing loading 


23 g/dm 2 (7.6 oz/ft 2 ) 


Max Airspeed 


20 kph 


Cruising Airspeed 


15 kph 


Engine 


EPS-300C 


Propeller 


EP-1080 


Thrust 


0.332Kg (11.1 oz) 


Power 


93 W 


Efficiency 


3.57 g/W 


Battery 


9.6V 750mAh Ni-MH 


Flight Time 


20 minutes 


Trailing antenna 


none 



Table 6.1: GWS E-Starter airframe specifications 
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Figure 6.1: The control system installed onto the E-Starter 
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Chapter 7 

Test Flight Results 



Once the airframe was completed, the magnetic navigation system was ground- 
tested in the laboratory (see Section 7.5). The next step is to test the control 
system on a real flight. 

7.1 Goals 

The testing intends to achieve three things. Firstly to test the control sys- 
tems ability to switch between autonomous mode and remotely piloted mode. 
Secondly to put the attitude control system through its paces to see how it 
responds to a different initial attitude as it attempts to command an atti- 
tude of its own. Thirdly, test the control systems performance in a variety 
of weather conditions. The flight time of the aircraft is approximately 20 
minutes so the goals are broken down and attempted over various flights. 
These goals would be tested according to the following schedule: 

• Ground test the control system, test the control system by moving the 
aircraft into various attitudes on the ground and see how it responds 
when the aircraft does roll, at the limits of the attitudes e.g. ±90° pitch, 
see if it looks right. Do the surfaces move in the correct direction in 
response to an error in attitude. Can the control system be reliably 
changed in and out of autonomous control. 

• Flight test the autonomous mode switching 

• When weather conditions are calm and dry, pilot the aircraft to an 
orientation which is close to the control systems desired (predetermined 
and hard coded by the user) value so that the error term is small and 
switch the aircraft into autonomous mode. Monitor whether level flight 
is maintained in the desired direction. 
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• Incrementally increase the initial heading error to 90 degrees over suc- 
cessive flights but maintain level flight so that the heading error is in- 
creased so the aircrafts ability to turn to the desired heading is tested. 

• Set the aircraft to an initial pitch toward the sky (negative pitch) by a 
small amount while the heading is approximately the desired value 

• Increase this pitch error to -45 degrees incrementally over successive 
flights. 

• Set the aircraft to an initial pitch toward the ground (positive pitch) by 
a small amount while the heading is approximately the desired value 

• Increase this pitch error to as much as possible by the airframe incre- 
mentally over successive flights. 

• increase the initial heading error to greater than 90 degrees up to 180 
degrees east and west over successive flights but maintain level flight 
so that the heading error is increased only then aircrafts ability to turn 
to the desired heading is tested. 

• Mix the two attitudes so that the control system starts with an attitude 
error which is orientated in both the pitch and heading directions. In- 
creasing the total rotation required by the control system to re-orientate 
the aircraft to the desired attitude. 

• Test the control systems ability as per above in less than perfect weather 
conditions, such as Gusty or light rain conditions. 



7.2 Method 

Due to deadlines (the due date for this project was just over seven months 
from its conception), and small windows of low-wind weather at the time of 
year of the testing (spring), it was important to be very productive when 
conditions where favorable. 

Testing required the help from several people, including camera people 
and a pilot, at a time of year which most students are very busy. Again, 
adding to the necessity for being productive during testing. Thus the follow- 
ing methodology was used: 

• Perform the calibration for the airframe 

• Trim airframe with control system on-board 
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• Select a site for the flight 

• Set clear goals for each test 

• Build on results when successful to increase difficulty for control system 

• Record results using video footage from the ground 

The implementation of this plan is laid out in the following sections. 

7.3 Trimming, Balancing and calibrating the 
airframe 

The calibration was performed as outlined in section 5.1.3 for each of the 
magnetometer axis. The gains and offsets calculated where: 

• X-axis: Offset = 2140, Gain = 1.21 

• Y-axis: Offset = 1854, Gain = 1 

• Z-axis: Offset = 2223, Gain = 1.2 

The control surfaces were then trimmed for zero deflection when the con- 
trol system commanded an angle of zero deflection. The Aircraft was then 
balanced on the ground approximately then flown via remote control with 
the flight system fitted to test for the correct balance. The CG was found 
to be too far back due to the magnetometer board located in the far end 
of the tail. The battery pack was moved forward to account for this and 
the plane retested. This showed it to be successfully balanced. During this 
successful balancing test flight the pilot trimmed the aircraft for level flight 
at 3/4 throttle. 

7.4 Select a location 

The location selected was the Dunedin Model Aero Club grounds at School 
Rd, Mosgiel. This site was selected because it is known to be airspace occu- 
pied by model sized aircraft and is in a rural area where radio interference 
would be unlikely on the frequency used for the remote pilot control (36.090 
MHz). Selecting some co-ordinates at regular intervals within 20km of the 
site and inputting the co-ordinates into the IGRF's (International Geomag- 
netic Reference Field) model shows that the magnetic field is fairly stable 
around the location: 

The test site had the following parameters: 
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Location 


Aero club 


Mosgiel town 


Caversham hill 


GPS Co-ordinates 


S 45deg 52.17293' 


S 45 53.29 


S 45 53.29 




E 170 15.90454' 


E 170 21.48 


E 170 28.29 


Altitude HAE 


12.1m 


10.0m 


30.0m 


Date 


2005.6191 


2005.6191 


2005.6191 


Declination 


24.434° 


24.453° 


24.466° 


Inclination 


-70.57° 


-70.566° 


-70.552° 


X component (nT) 


17,789.55 


17,788.33 


17,794.18 


Y component (nT) 


8,082.39 


8,088.99 


8,096.38 


Z component (nT) 


-55,394.1 


-55,384.95 


-55,366.62 


Horizontal intensity (nT) 


19,539.53 


19,541.15 


19,549.53 


Total intensity (nT) 


58,739.25 


58,731.16 


58,716.66 



Table 7.1: Magnetic field information near Dunedin 



• GPS location: S 45deg 52.17293', E 170 15.90454' 

• Magnetic field vector: X component = 17,789.55 nT, Y component = 
8,082.39 nT, Z component = -55,394.1 nT 

• Type of airspace: NOTAM'd (NOtice To AirMen) as model aero club 
space, where model aircraft are likely. The airspace is also used for low 
level flight training operations by Mainland Aviation College, Massey 
University School of Aviation, and Otago Aero Club. 

• Time of flights: Between 5pm and 7pm due to the pilots working hours. 

• Altitude (HAE (height above ellipsoid)): 12.1m 

• Weather conditions: Variable, inland several kilometers so tends to 
measure lower airspeeds and the location being near the Dunedin air- 
port meant that updated weather information such as wind-speed and 
rainfall where readily available. 

• Terrain and visibility: Surrounded by small hills with a large flat plain 
about 5km in radius of rural paddocks and low in altitude meant that 
provided the weather conditions were favorable the visibility would be 
likely to be very large and cloud base would be a decent distance above 
the intended operating altitudes. 
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7.5 Results 



Day one (27/9/05): The aim on day one was to perform the calibration steps 
and to balance and trim the airframe. Alan Coppard kindly volunteered to 
act as our test pilot. Alan is a highly skilled RC pilot. 

• Calibration, performed for magnetometers as per section 5.1.3. The 
values were updated on location using a laptop set up as pictured in 
7.1. 

• Trim, This was set for the remote pilots controller so that the au- 
tonomous mode is switched on when the transmitters fourth channel 
control lever is centered. Also the elevator and rudder trim was ad- 
justed so that with the controls centered and 3/4 throttle the aircraft 
would maintain level flight. 

• Throttle. The throttle required to fly the aircraft at a reasonably low 
angle of attack of around 5 degrees was found to be about 3/4 throttle. 
So the control systems commanded throttle for the test flights was 
increased from 1/2 to 3/4 throttle. 

• Weight, the center of gravity (CG) was found to be to far back. The 
battery pack was moved further forward to compensate for the magne- 
tometer sensors being located at the far end of the long tail (near the 
rudder) . 

• Duration of flight, 10 minutes 
Day two (29/9/05): 

• Trim, The Magnetometer board was rotated in pitch so that the eleva- 
tor attempted to hold a small amount of pitch while the plane was held 
level on the ground. When the system was engaged into autopilot in 
the air the aircraft remained relatively level but the angle of attack was 
not sufficient for the constant airspeed selected. So the magnetometer 
board was rotated on its mount slightly, which involves unpinning the 
polystyrene block which it is mounted on and re-pinning the block after 
pitching the magnetometer board down. 

• The throttle was decided to be still slightly low also so this was in- 
creased to 80 percent. 

• Weight adjustment seemed to have been successful. 
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Figure 7.1: The ground station setup for updating the firmware, offsets, and 
gains in the field 

• Successful changing between modes while airborne (RPV/Autonomous) 

• Successful heading hold, attitude stabilization 

• Successful turn toward desired heading (north from west) approxi- 
mately 20 degrees to start with then increased over three attempts 
to approximately 90 degrees. Stable, level flight in the correct head- 
ing was quickly regained in approximately 5 seconds from 90 degrees 
heading error. See photo sequence below. 

• Successful remote control takeoff and landings. The pilot and airframe 
have at this point successfully completed several takeoff and landings 
while under remote pilot control. 

• Successful footage of testing was taken, There is a need for lower flight 
and more tests to be able to assess oscillation, from current footage 
plane seems to be excellent with plenty of airframe damping. Even 
in slight gusts of approximately lOkmh which is comparable to the 
aircrafts 20kmh airspeed, which can be seen in the video footage. 

• The biggest problem for the testing is the poor range of the R/C trans- 
mitter which is quoted to 1.5km but only achieves a maximum of less 
than 50m. This range is far too small, the plane becomes uncontrol- 
lable by the remote pilot outside of this range and the safety feature 
implemented in this version of the flight controller causes the aircraft 
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to default into remote control which is just noise picked up by the re- 
ceiver so the plane looses control and turns off its throttle causing the 
aircraft to crash. This range needed to be increased in order to fully 
assess the control system over a longer duration. 

• Duration of flight 20 minutes 
Day three (2/10/05): 

• The range problem necessitated the change of the radio equipment. 
This involved swapping the receiver which plugs into the main board 
and using a different higher powered transmitter. This was an oppor- 
tunity to see how the system would cope with the change in equipment, 
assessing its ability to work with off the shelf radio control components. 
The result was a success and the range was increased to over 1km even 
though the transmitters battery was less than completely full so flight 
testing could commence. 

• Hand launching the aircraft was successful and footage was obtained 
so that an algorithm can be developed to remove the pilot needing to 
initially take off the aircraft. 

• The control system was now ready to try and recover the aircraft from 
first a climb and then a dive attitude. The aircraft was faced north 
and put into a climb of approximately 20 degrees and then switched 
into autonomous mode. The correct attitude of wings level and flying 
North was achieved within two seconds of being engaged. So a dive was 
attempted. Shallow to start with, only 20 degrees down, and this was 
also recovered within two seconds. So the plane was climbed to a safe 
altitude once again and dived toward the ground at approximately 45 
degrees from which it recovered in approximately 3 seconds as seen in 
the video footage of the introduction clip. The control system has not 
yet implemented the designed airspeed sensor so there is no feedback 
for the velocity of the aircraft. This causes an oscillation in pitch 
recovering from the dive where airspeed has been increased since as 
discussed earlier the amount of control required to change the aircrafts 
state varies with air speed. This oscillation would be dampened out 
if the integral and proportional parts (as per section 5.4.1)of the P1D 
where implemented. This effect was not really found to increase at 
steeper pitch angles due to the slow terminal velocity of the airframe. 

• Now it was time to test the control systems ability to adjust the head- 
ing for errors greater than 90 degrees. The aircraft was flown south 
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west when the on-board controller was requesting level north flight and 
then autonomous mode was selected. The aircraft was known from 
ground testing that it would attempt some sort of wing over maneuver 
to attempt to rotate to the desired attitude. The control input seemed 
to be satisfactory; a little aggressive but for the chosen airframe is a 
valid maneuver. At larger heading errors however, closer to 180 de- 
grees the current control system attempts to smoothly put less control 
into the rudder so that the transition from a left or right turn by the 
rudder is smooth. This is necessary since at 180 degrees error the con- 
trol system would request maximum rudder left or right other wise. 
Often rapidly flicking between both, until a small rotation away from 
this case is achieved. Commanding the servos in this way causes them 
to drain a large amount of current and could have caused the control 
system to reset due to low voltages, caused by the internal resistance of 
the batteries and speed controller not being able to supply the required 
current. So the rotation requested at exactly 180 degrees heading is a 
pitch upwards. This pitch upwards caused the aircraft to stall since 
the throttle is not actively adjusted and the aircraft yaws to the left or 
right. The change in heading sufficiently calculates a rotation for the 
heading about the z axis and commands increasing amounts of rudder. 
This process looks like a messed up wing over but does successfully 
bring the plane around to the desired heading with wings level without 
loosing any height. It is a rare situation for the control system to be 
flying 180 degrees away from the desired direction of travel but this 
situation could occur if a waypoint was requested of the current con- 
trol system which was directly behind the aircrafts current direction of 
flight. So it was necessary to test. 

• It was time to test the planes response to an arbitrary initial attitude 
state and record its response to attempt to fly the aircraft wings level 
and heading North. The response of the control system about the 
combined y and z axis was the same as the aircrafts response about 
each of the axis individually. Pitch rotations about the y axis were 
more responsive than heading rotations about the z axis as would be 
expected. 

• The control system has received a variety of wind and rain conditions 
over span of the testing period. It's response to gusts tends to be 
a translational slip as the system manages to maintain the heading 
and pitch directions relatively stably. The aircraft can be seen flying 
in light rain successfully in the footage of this day's (day 3) testing 
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Figure 7.2: A series of still images from video footage showing the transition 
from pilot control to autonomous control. The frames read from left to right, 
top to bottom. They are taken at regular intervals every 18 frames which 
represents approximately 0.71s depicting a controlled turn toward north. 

without much difficulty This means that a flight does not necessarily 
need to be aborted due to rain which is advantageous. 

• Total duration of flights, 60 minutes. Total flights on the day, 4 
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Chapter 8 
Conclusions 



Apeliotes has been successful in magnetometer based attitude control. The 
control system successfully regained control to level flight with the desired 
heading (geographical North). A pilot on the ground commanded various 
initial orientations of pitch and heading from which the control system had to 
recover. The first autonomous flight was achieved on 29/9/05, seven months 
after the projects conception. A GWS E-Starter model aircraft was fitted 
with standard low cost radio control hobbyist equipment and the Apeliotes 
autonomous flight controller. 

As described in this paper, the controller used only a three axis magne- 
tometer sensor to determine an attitude solution in pitch and heading with 
the assumption of zero roll. This assumption proved to be valid for the 
control system as the a roll motion turned out to couple through the mea- 
surements in such a way that the system remained stable at all attitudes 
including rolls so that the plane attempts to zero the roll using control itself. 
In a knife edge position the aircraft will attempt to yaw the plane so that it 
is nose up where the system can now apply elevator to force the pitch to zero 
so that the wings are now level. If upside down, the control system attempts 
to loop back to the desired attitude. So the zero roll assumption does not 
cause the aircraft to loose control when there is roll present. 

On the third day of testing the longest and most difficult flight of the 
testing session was accomplished. The aircraft was initially put in a dive of 
45 degrees toward the ground at an altitude of 20m above the ground heading 
90 degrees off north. The control system successfully regained control and 
turned to make the required rotation of yaw and pitch to fly in the requested 
direction of geographic north for a distance of approximately 100m before 
being switched back out of autonomous mode to continue testing. 

A solution seems to be possible about the roll axis by using more magne- 
tometers but this will need to be investigated further. Along with numerous 
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upgrades some of which are listed below, in the Section labeled further work. 

8.1 Further work 

This project is very extensible. The functionality which follows is intended 
to be implemented into Apeliotes in the future. These could be adopted by 
anyone for research. 

8.1.1 Coping with noise and inaccuracy 

Noise and inaccuracy is introduced at many stages such as the sensors, am- 
plification, sampling, data storage and processing etc. These sources of noise 
can be overcome using filtering and appropriate choices of units to represent 
numbers. 

• Hardware filtering: Butterworth filtering, active filtering, digital links. 

• Software Filtering: Kalman Filtering. Extended Kalman filtering 

8.1.2 Number representation 

an attempt has been taken to represent numbers with units so as to produce 
a value close to one. This reduces inaccuracies introduced from overflow and 
underflow during mathematical computations. More time should be taken to 
further implement this throughout the code to increase accuracy now that 
the initial code has proved to work. Any further work should maintain this 
policy. 

8.1.3 Further development 

• Long range telemetry 

• 3D Simulator port 

• HUD GUI from telemetry 

• Topographical Flight planning interface 

• Higher order control: having completed the first order approach suc- 
cessfully it is time to consider higher order control utilizing the available 
information.... 

• Altitude hold: Controlling Rate of Climb, Controlling Altitude 
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simple take-off: button on side revs engine to max rpm then level throw 
in direction letting go initializes level climb to some altitude when it 
switches into flight plan to do waypoints.... 

maintaining optimum airspeed: feedback of airspeed measurement 

Optimized turning: using accelerometers for turn coordinating.... gy- 
ros 

Simple landing: engine throttle decrease with level decent, path given 
by input waypoints.... 
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Appendix A 
Quaternions 



Quaternions are a vector containing 4 scalar variables; quaternions can be 
added and multiplied as a normal number. There is however a difference, 
unlike the algebra of scalar numbers quaternion multiplication is not com- 
mutative. Quaternions have 4 dimensions with 4 variables. One dimension 
is real and the other 3 are imaginary dimensions. Each of the imaginary 
dimensions has a unit value equal to the square root of -1, but are different 
square roots of -1, each mutually perpendicular to each other, denoted as i, 
j and k. Since quaternions work in 4 dimensions there are at least 3 ways to 
get from +1 to -1. Quaternions take the form Q = a + ib + jc + kd . 

Quaternions are useful for representing rotations in three dimensions 
(3D). To do this the quaternions are normalized so that they have unit mag- 
nitude. Once normalized, multiplications are used to represent a combination 
of different rotations once normalized. 

Quaternions used in this way can be thought of as being similar to axis- 
angles except that the real part is equal to cos(^) (where u is the angle of 
rotation) and the complex parts are made up of the axis vector times sm(|). 
The angular representation of the quaternion is, 

q = cos(-) + i(x * sin(—)) + j(y * sin(—)) + k(z * sin(—)) 

where u is the angle of rotation and x, y, and z represent the components 
of the vector about which the rotation occurs. So quaternions are closely 
related to the axis-angle representation of rotations. 

The quaternion i represents a rotation of 180 degrees about the x-axis, 
the quaternion j represents a rotation of 180 degrees about the y-axis, and 
the quaternion k represents a rotation of 180 degrees about the z-axis. So 
% * % — — 1 represents a rotation of 360 degrees about the x-axis. 

The negative of a quaternion represents the same rotation. So if (w + 
xi + yj + zk) represents a rotation then (— w — xi — yj — zk) represents the 
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same rotation although it is a different quaternion. The rotation is the same 
because the angle is reversed but also the axis is reversed. 

Quaternions are spinors. An object which, if rotated by 360 degrees is 
inverted, is known as a spinor. Spinors provide a means of representing 
rotations in n dimensions. They were first defined by physicists working in 
the field of quantum mechanics. Spinors in four-dimensional space occur in 
Dirac's equations for the wave functions of an electron. 

A.l Multiplying quaternion numbers 

The multiplication rules for quaternions are: 

i x i = jxj = kxk = -l 

i x j = k 

j x i = —k 

j x k = i 

k x j = —i 

k x i = j 

i x k = —j 

Where quaternion 1, zl = (a + ib + jc + kd) and quaternion 2, z2 = (e + if + 
jg + kh) 

The multiplication of quaternions expands as follows: 
(a + ib + jc + kd) x (e + if + jg + kh) 



a(e + if + jg + kh) 
+ ib(e + if + jg + kh) 
+ jc(e + if + jg + kh) 
+ kd(e + if + jg + kh) 



axe + iaxf+jaxg + kaxh 

+ ibxe + ixixbxf + ixjxbxg + ixkxbxh 

+jcxe+jxixcxf+jxjxcxg+jxkxcxh 

+kdxe+kxixdxf+kxjxdxg+kxkxdxh 
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axe + iaxf+jaxg + kaxh 

+ ibxe-bxf + kxbxg-jxbxh 

+ jcxe — kxcxf — cxg + ixcxh 

+kdxe+jxdxf-ixdxg-dxh 

axe-bxf-cxg-dxh 
+ i(bxe + axf + cxh — dxg) 
+ j(a x g — bxh + cxe + dxf) 
+ k(a xh + bxg — cxf + dxe) 

The result is: 

zl x z2 = 

axe — bxf — cxg — dxh + 
i(bxe + axf + cxh-dxg) + 
j(a xg-bxh + cxe + dxf) + 
k(a xh + bxg-cxf + dxe) 
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Appendix B 
Significant code 



The following code is written in C. It 
significant parts of the control system, 
complete fixed point math library. The 
16.16 bits. 



shows some excerpts from the most 
The Fixedl6 declaration is part of a 
format for a Fixed 16 number here is 



B.l Reference field rotation 

Fixedl6 qrO = cos (m_roll/2) *cos (m_pitch/2) *cos (m_yaw/2) + sin(m_roll/2) *sin(m_pitch/2) *sin(m_yaw/2) ; 
Fixedl6 qrl = sin(m_roll/2) *sin(m_pitch/2) *cos (m_yau/2) - cos (m_roll/2) *sin(m_pitch/2) *cos (m_yaw/2) ; 
Fixedl6 qr2 = cos (m_roll/2) *sin(m_pitch/2) *cos (m_yau/2) + sin(m_roll/2) *cos (m_pitch/2) *sin(m_yaw/2) ; 
Fixedl6 qr3 = cos (m_roll/2) *cos (m_pitch/2) *sin(m_yau/2) - siriCm_roll/2) *sin(m_pitch/2) *cos (m_yaw/2) ; 



Fixedl6 two (2) ; 



Fixedl6 x_field = qrO*qrO*trueN_f ield.x + two*qr2*qrO*trueN_f ield.z - two*qr3*qrO*trueN_f ield.y + 
qrl*qrl*trueN_f ield.x + two*qr2*qrl*trueN_f ield.y + two*qr3*qri*trueN_f ield.z - 
qr3*qr3*trueN_f ield.x - qr2*qr2*trueN_f ield.x; 

Fixedl6 y_field = two*qrl*qr2*trueN_f ield.x + qr2*qr2*trueN_f ield.y + two*qr3*qr2*trueN_f ield.z + 
two*qrO*qr3*trueN_f ield.x - qr3*qr3*trueN_f ield.y + qrO*qrO*trueN_f ield.y - 
two*qrl*qrO*trueN_f ield.z - qrl*qrl*trueN_f ield.y; 



Fixedl6 z_field = two*qrl*qr3*trueN_f ield.x + two*qr2*qr3*trueN_f ield.y + qr3*qr3*trueN_f ield.z - 
two*qrO*qr2*trueN_f ield.x - qr2*qr2*trueN_f ield.z + two*qrO*qri*trueN_f ield.y - 
qrl*qrl*trueN_f ield.z + qrO*qrO*trueN_f ield.z; 

//set the global variables 
vars->x_vect = x_field; 
vars->y_vect = y_field; 
vars->z_vect = z_field; 



B.2 Map measured field to reference field 

Fixedl6 qO = -trueN_field.x*observed_f ield.x - trueN_f ield.y*observed_f ield.y- trueN_field.z*observed_f ield.z; 
Fixedl6 ql = trueN_field.y*observed_f ield.z - trueN_field.z*observed_f ield.y; 
Fixedl6 q2 = -trueN_field.x*observed_f ield.z + trueN_field.z*observed_f ield.x; 
Fixedl6 q3 = trueN_f ield.x*observed_f ield.y - trueN_field.y*observed_f ield.x; 



64 



B.3 



Rotation from measured to reference mag- 
netic field for attitude 



Fixedl6 qO = -trueN_f ield.x*observed_f ield.x - trueN_f ield.y*observed_f ield.y- trueN_f ield.z*observed_f ield.z; 
Fixedl6 ql = trueN_field.y*observed_f ield.z - trueN_f ield.z*observed_f ield.y; 
Fixedl6 q2 = -trueN„field.x*observed_f ield.z + trueN_field.z*observed_f ield.x; 
Fixedl6 q3 = trueN_field.x*observed_f ield.y - trueN_f ield.y*observed_f ield.x; 



B.4 Proportional control 

inline FixedVector calc_control C) 
{ 

Fixedl6 pitch_gain(Fixedl6(2)) ; 
Fixedl6 yaw_gain(Fixedl6(7) ) ; 

Fixedl6 roll_error(vars->roll_vect) ; // radians 
Fixedl6 pitch_error(vars->pitch_vect) ; // radians 
Fixedl6 yaw_error (vars->yaw_vect) ; // radians 



//NED reference frame with the right hand rule for rotations 
Fixedl6 set.aileron = Fixedl6(45); 

Fixedl6 set_elevator = Fixedl6 (- ( (pitch_error*pitch_gain*2*45) / pi) + 45); 
Fixedl6 set_rudder = Fixedl6( ( (yaw_error*yaw_gain*45) / pi) + 45); 

return FixedVector(set_aileron, set_elevator, set_rudder) ; 



B.5 Navigation 

f ixed32 GC.distance (f ixed32(latl) , f ixed32(lonl) , f ixed32 (lat2) , f ixed32(lon2) , fixed32 (altitude) ) 
{ 

f ixed32 (radian_dist) ; 

f ixed32(earths_radius) = 6366710; 

//radian distance on the earths surface. 

return 2*asin(sqrt ( (sin( (latl-lat2) /2) ) "2 + cos (latl)*cos (lat2) * (sin( (lonl-lon2) /2) ) "2) ) ; 
//distance in meters 

//return (altitude + earths_radius) * radian_dist; 

> 

fixed32 DMS_to_rad(fixed32 (degrees) , fixed32 (minutes) , f ixed32(seconds) ) 
{ 

return (degrees + minutes/60 + seconds/3600) * (Fixedl6 :: PI ()) /180 ; 
} 



//however this is NW positive signs 

fixed32() true_course (f ixed32(latl) , f ixed32 (lonl) , f ixed32(lat2) , f ixed32 (lon2) , fixed32 (altitude) ) 
{ 

fixed(d) - GC_distance(latl, lonl, lat2, lon2, altitude); 
//special case of starting at the poles 

if(cos(latl) < EPS){ // EPS a small number ~ machine precision 
ifUatl > 0) 

return (Fixedl6: :PI()) ; // starting from N pole 

else 

return 2* (Fixedl6 : : PI () ) ; // starting from S pole 

} else { 

//For starting points other than the poles: 

if( sin(lon2-lonl)<0) 

return acos ( (sin(lat2) -sin(latl) *cos (d) ) / (sin(d) *cos (latl) ) ) ; 

else 

return (2*pi-acos ( (sin(lat2)-sin(latl) *cos (d) ) / (sin (d) * cos (latl) ) ) ) ; 
// //alternative formula not requiring the computation the distance between the points is: 
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// return mod(atan2 (sin(lonl-lon2) *cos flat 2) , 

// cos (latl) *sin(lat2)-sin(latl)*cos(lat2)*cos(loni-lon2)) , 2*pi) ; 

> 

> 



void init_navigat ion (void) { 
struct uav_vars* vars = VAR_ADDR; 

vars->waypoint_ptr = 0; //next waypoint to achieve is at position 0 in the waypoint array 

// waypoints 

//first waypoint: 

vars->uaypoints [0] [0] = 0; //latitude (radians) 

vars-> waypoints [1] [0] = DMS_to_rad(170, 15.90454, 0); //longitude (radians) 
vars->waypoints[2] [0] - 30.1; //altitude HAE (m) 

// 

vars->GPS_ptr - 0; 
} 
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